#include "virtual_camera/camera_node.hpp"


CameraNode::CameraNode(const rclcpp::NodeOptions &options)
: Node("virtual_camera", options) {
    // 设置参数
    fps = this->declare_parameter("fps", 30);
    video_path = this->declare_parameter("video_path", "video.mp4");

    // 初始化相机
    cam = std::make_shared<VirtualCamera>(video_path);
    if (cam->open() != 0) {
        RCLCPP_ERROR(this->get_logger(), "视频读取失败");
        return;
    } else {
        RCLCPP_INFO(this->get_logger(), "虚拟相机启动成功");
        RCLCPP_INFO(this->get_logger(), "视频文件: %s", video_path.c_str());
        RCLCPP_INFO(this->get_logger(), "帧率: %d", fps);
    }

    // 初始化图像发布器
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/image_raw", 10);

    // 初始化计时器
    timer_ = this->create_wall_timer(std::chrono::milliseconds(1000 / fps), std::bind(&CameraNode::getImage, this));
}

void CameraNode::getImage() {
    // 获取一帧图像
    cv::Mat frame = cam->getFrame();
    rclcpp::Time stamp = this->now();
    if (frame.empty()) {
        RCLCPP_ERROR(this->get_logger(), "图像获取失败");
        return;
    }

    // 使用cv_bridge转换为ROS消息
    std_msgs::msg::Header header;
    header.set__frame_id("camera_frame");
    header.set__stamp(stamp);
    
    auto image_msg = cv_bridge::CvImage(header, "bgr8", frame).toImageMsg();
    
    // 发布消息
    image_pub_->publish(*image_msg);
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(CameraNode)
